#include "thread_node.h"

#include <cv_bridge/cv_bridge.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/image_encodings.h>

#include <condition_variable>
#include <mutex>
#include <opencv2/opencv.hpp>
#include <queue>
#include <thread>

std::queue<PDATA> PQUEUE;
std::queue<PDATA> PQUEUE_A;
std::queue<CDATA> CQUEUE;
std::queue<CDATA> CQUEUE_B;
std::mutex mtx;
std::condition_variable cvv;

int queue_size = 20;

void laserCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
  PDATA pdata;
  pdata.timestamp = msg->header.stamp;
  pdata.point_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg(*msg, *pdata.point_cloud);

  // print pointcloud size
  ROS_INFO("laserCallback PDATA size: %d Timestamp: %f",
           pdata.point_cloud->size(), pdata.timestamp.toSec());

  std::lock_guard<std::mutex> lock(mtx);
  // check PQUEUE size ,if size >queue_size, delete front
  if (PQUEUE.size() >= queue_size) {
    PQUEUE.pop();
  }
  PQUEUE.push(pdata);
  cvv.notify_all();
}

void imageCallback(const sensor_msgs::Image::ConstPtr& msg) {
  CDATA cdata;
  cdata.timestamp = msg->header.stamp;
  cdata.image = *msg;

  ROS_INFO("imageCallback CDATA Timestamp: %f", cdata.timestamp.toSec());

  // show image
  cv_bridge::CvImagePtr cv_ptr;
  try {
    cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }
  // cv::imshow("Image window", cv_ptr->image);
  // cv::waitKey(10);

  std::lock_guard<std::mutex> lock(mtx);
  if (CQUEUE.size() >= queue_size) {
    CQUEUE.pop();
  }
  CQUEUE.push(cdata);
  cvv.notify_all();
}

void threadA() {
  // ros::Rate rate(5);

  while (true) {
    std::unique_lock<std::mutex> lock(mtx);
    cvv.wait(lock, [] { return !PQUEUE.empty(); });

    PDATA pdata = PQUEUE.front();
    PQUEUE.pop();
    lock.unlock();

    // Voxel downsampling
    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
    voxel_grid.setInputCloud(pdata.point_cloud);
    voxel_grid.setLeafSize(0.1, 0.1, 0.1);
    pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled(
        new pcl::PointCloud<pcl::PointXYZ>);
    voxel_grid.filter(*downsampled);
    // print pointcloud size before voxel filter and after voxel filter
    // std::cout << "PointCloud before voxel filter: "
    //           << pdata.point_cloud->points.size() << " data points."
    //           << std::endl;
    // std::cout << "PointCloud after voxel filter: "
    //           << downsampled->points.size() << " data points." << std::endl;

    PDATA pdata_a;
    pdata_a.timestamp = pdata.timestamp;
    pdata_a.point_cloud = downsampled;

    std::lock_guard<std::mutex> lock_a(mtx);
    if (PQUEUE_A.size() >= queue_size) {
      PQUEUE_A.pop();
    }
    PQUEUE_A.push(pdata_a);

    // rate.sleep();
  }
}

void threadB() {
  // ros::Rate rate(10);

  while (true) {
    std::unique_lock<std::mutex> lock(mtx);
    cvv.wait(lock, [] { return !CQUEUE.empty(); });

    CDATA cdata = CQUEUE.front();
    CQUEUE.pop();
    lock.unlock();

    // Image resizing
    cv::Mat image;
    cv_bridge::CvImagePtr cv_img;
    try {
      cv_img =
          cv_bridge::toCvCopy(cdata.image, sensor_msgs::image_encodings::BGR8);
      image = cv_img->image;
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      continue;
    }

    cv::Mat resized_image;
    cv::resize(image, resized_image, cv::Size(320, 240));
    // show image
    cv::imshow("image", resized_image);
    cv::waitKey(10);

    CDATA cdata_b;
    cdata_b.timestamp = cdata.timestamp;
    cdata_b.image =
        *cv_bridge::CvImage(std_msgs::Header(), "bgr8", resized_image)
             .toImageMsg();

    std::lock_guard<std::mutex> lock_b(mtx);
    if (CQUEUE_B.size() >= queue_size) {
      CQUEUE_B.pop();
    }
    CQUEUE_B.push(cdata_b);

    // rate.sleep();
  }
}

void threadC() {
  ros::Rate rate(10);

  while (ros::ok()) {
    std::unique_lock<std::mutex> lock(mtx);
    cvv.wait(lock, [] { return !PQUEUE_A.empty() || !CQUEUE_B.empty(); });

    // print queue size
    std::cout << "A: " << PQUEUE_A.size() << " B: "
              << CQUEUE_B.size() << std::endl;

    if (!PQUEUE_A.empty()) {
      bool is_lidar_camera_match = false;

      // get data
      PDATA pdata ;
      CDATA cdata ;

      if ((PQUEUE_A.front().timestamp.toSec()+0.05) <
          CQUEUE_B.front().timestamp.toSec()) {
        pdata = PQUEUE_A.front();
        PQUEUE_A.pop();
      } else {
        pdata = PQUEUE_A.front();
        PQUEUE_A.pop();

        // for遍历CQUEUE_B
        float interval_time = 100;
        int interval_id = 0;
        int CQUEUE_B_size = CQUEUE_B.size();
        std::queue<CDATA> CQUEUE_B_tmp = CQUEUE_B;

        for (int i = 0; i < CQUEUE_B_size; i++) {
          CDATA cdata_tmp = CQUEUE_B_tmp.front();
          CQUEUE_B_tmp.pop();
          if (cdata_tmp.timestamp.toSec() - pdata.timestamp.toSec() <
              0.05) {
            if (interval_time > abs(cdata_tmp.timestamp.toSec() -
                                    pdata.timestamp.toSec())) {
              interval_time = abs(cdata_tmp.timestamp.toSec() -
                                  pdata.timestamp.toSec());
              // print
              // ROS_INFO("interval_time: %f", interval_time);
              // ROS_INFO("interval_id: %d", i);
              //获取当前数据的索引
              interval_id = i;
            }
          }
        }
        if (interval_time < 0.05)
        {
          is_lidar_camera_match = true;
        }
        for (int i = 0; i < CQUEUE_B_size; i++) {
          if (i == interval_id)
          {
            cdata = CQUEUE_B.front();
            break;
          } else {
            CQUEUE_B.pop();
          }
        }
      }
      lock.unlock();

      if (is_lidar_camera_match) {
        ROS_INFO(
            "///////////////Match PDATA Timestamp: %f - CDATA Timestamp: "
            "%f",
            pdata.timestamp.toSec(), cdata.timestamp.toSec());
      } else {
        ROS_INFO("///////////////NoMatch  PDATA Timestamp: %f",
                 pdata.timestamp.toSec());
      }

    } else {
      lock.unlock();
    }
    rate.sleep();
  }
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "your_project_name");
  ros::NodeHandle nh;

  ros::Subscriber laser_sub =
      nh.subscribe("/zvision_lidar_points", 10, laserCallback);
  ros::Subscriber image_sub =
      nh.subscribe("/camera/rgb/image_raw", 10, imageCallback);

  std::thread thread_a(threadA);
  std::thread thread_b(threadB);
  std::thread thread_c(threadC);
  // thread_c.join();

  ros::Rate rate(100);
  while (ros::ok()) {
    ros::spinOnce();
    rate.sleep();
  }

  return 0;
}